function shock_smallwood_cm(first, last, dataset)
%Created to collect the Hershfeld/Smallwood functions
srs_analysis(first, last, dataset);

%----------------------------------------------------------------
function [srs,fn]=srs_analysis(first,last, dataset)
global Project;

% w = waitbar(0,'Please wait...');

% Determine Which Analysis index to save Data to.
% try
%	n = length(Project.Test_Item.Run.Channel(1).Analysis);
%	n = n + 1;
% catch
%	n = 1;
% end
w = waitbar(0,'Please wait...');

for i = 1:Project.Test_Item.Run.Num_Chan
    fs=Project.Test_Item.Run.Sample_Rate;
    x=Project.Test_Item.Run.Channel(i).Time_His(first:last);
    [srs,fn]=shspec(x,[],[],fs);
    Project.Test_Item.Run.Channel(i).Analysis(dataset).Type = 6;
    Project.Test_Item.Run.Channel(i).Analysis(dataset).Abs = fn;
    Project.Test_Item.Run.Channel(i).Analysis(dataset).Ord = srs;
    Project.Test_Item.Run.Channel(i).Analysis(dataset).FPoint = first;
    Project.Test_Item.Run.Channel(i).Analysis(dataset).LPoint = last;
    waitbar(i/Project.Test_Item.Run.Num_Chan);
end
close(w);

%----------------------------------------------------------------
function [s,fn]=shspec(x,fn,z,sr,itype,bwt,awt)
% shspec.m
% function [s,fn] = shspec(x,fn,z,sr,itype,bwt,awt)
%
% Function to calculate the shock response spectrum using a ramp 
% invarient digital filter simulation of the single-degree-of-freedom
% -system.
%
% INPUT:   x = a vector containing the input time history
%         fn = a vector containing the frequencies at which the spectrum
%              will be calculated.
%              Default: fn=logspace(log10(sr/1e4),log10(sr/4),50);
%          z = the fraction of critical damping for the spectrum
%              calculation. Default: z=.05
%         sr = the sample rate of the time history, x. Default: sr=1
%      itype = the type of spectrum desired
%            > 0 (pos)-- base acceleration-absolute acceleration model
%            < 0 (neg)-- base acceleration-relative displacement model
%                   (expressed in equivalent static acceleration units)
%            If abs(itype) is:
%            1--positive primary,  2--negative primary,  3--absolute maximum primary
%            4--positive residual, 5--negative residual, 6--absolute maximum residual
%            7--largest of 1&4, maximum positive, 8--largest of 2&5, maximum negative
%            9 -- maximax, the largest absolute value of 1-8
%           10 -- returns a matrix s(9,length(fn)) with all the types 1-9.
%            Default: itype=9
%      bwt,awt = optional weights length(fn)x3. If not given, computed by sdofwt.m 
% All input arguments except the first are optional.
% If an input is empty or missing the default value is used.
%
%  OUTPUT: s = a matrix containing the shock response spectrum at the
%              frequencies, fn.  If abs(itype)<10, s will have 1 row
%              If abs(itype)==10 each row of s will contain 1 spectrum
%              type for all freq's; each column will contain all types for one freq.
%          fn = the natural frequencies at which the shock spectrum is calculated.

%
%  other routines used: sdofwt -- designs the filters
%                       sdoffltr -- filters a time history with a sdof
%                                   filter
%                       shres -- finds the residual response
%
% Reference: 1. Smallwood D.O.,"An Improved Recursive Formula
%               for Calculating Shock Response Spectra," The
%               Shock and Vibration Bulletin 51(2), pp 211-217,
%               May 1981.
%            2. Smallwood D.O.,"The Shock Spectrum at Low Frequencies,"
%               The Shock and Vibration Bulletin 56(1), Appendix A,
%               pp 285-287, Aug 1986.
%
% D. O. Smallwood, Sandia National Laboratories, 19 June 1989.
% Modified 8-4-89 to add itype=10
% Added optional input weights bwt,awt 3/4/94 dos
% Added error checking, defaults, and 2nd output variable, 6/8/98, dos
% Copywrite 1994-2000 Sandia National Laboratories

% set defaults
if nargin<5, itype=9; end
if isempty(itype), itype=9; end
if nargin<4, sr=1; end
if isempty(sr), sr=1; end
if nargin<3, z=.05; end
if isempty(z), z=.05; end
% if nargin<2, fn=logspace(log10(sr/1e4),log10(sr/4),50); end
% if isempty(fn),fn=logspace(log10(sr/1e4),log10(sr/4),50); end
% Edited on 24-Aug-2010 by Chris McLeod to provide 1/12 octave frequency
% spacing for the SRS.

if nargin<2, fn=FreqOctave((sr/1e4),(sr/4),12.0); end
if isempty(fn),fn=FreqOctave((sr/1e4),(sr/4),12.0); end
   
nf=length(fn);
nx=length(x);
if nf==0
   disp('Error in shspec: length of vector of natural frequencies, fn==0')
   return
end
if nx==0
   disp('Error in shspec: length of input vector x is zero')
   return
end
xmax = max(abs(x));
jtype=abs(itype);
   if xmax<=eps        % special case when input is zero
      if jtype < 10, s(1,:) = zeros(1,nf); end
      if jtype == 10, s = zeros(9,nf); end
      return
   end

xx = x(:)';                   % make sure time history is a row vector
error='error in shspec';
if jtype>10, fprintf('%s\n', error); fprintf('%d\n',itype); end
if itype==0, fprintf('%s\n', error); fprintf('%d\n',itype); end
if sr<=0, fprintf('%s\n', error); fprintf('sr = %f\n', sr); end
if min(fn)<=0, fprintf('%s\n', error); fprintf('min(fn) = %d\n', min(fn)); end
if z<=0, fprintf('%s\n', error); fprintf('z = %f\n', z); end
if jtype<10, ns=1; end
if jtype==10, ns=9; end
s=zeros(ns,nf);

% loop for each natural frequency
for i=1:nf
    if nargin<7
       [b,a] = sdofwt(fn(i),sr,z,itype);
    elseif isempty(bwt)|| isempty(awt)
       [b,a] = sdofwt(fn(i),sr,z,itype); 
    else
       b = bwt(i,:); a = awt(i,:);
    end
%   append enough zeros to get 1/100 of the residual response
    nzeros = max(2, ceil(.01*sr/fn(i)) );
    xe = [xx zeros(1,nzeros)];

%   filter the appended input to find the filter output at one natural frequency
    y =  sdoffltr(b,a,xe);
%disp('shspec: line 102'), keyboard

%   find primary response

    primax=max(y(1:nx));
    primax=max(0,primax);
    primin=min(y(1:nx));
    primin=abs(min(0,primin));
    priabs=max(primax,primin);

%   find residual response

    t(1)=0;
    t(2)=(nzeros-1)/sr;
    zy=zeros(1,2);
    zy(1)=y(nx+1);
    zy(2)=y(nx+nzeros);
    res=zeros(1,2);
    res = shres(zy,t,fn(i),z);
%disp('shspec: line 120'), keyboard
    respos=max(res);
    respos=max(0,respos);
    resneg=min(res);
    resneg=abs(min(0,resneg));
    resabs=max(respos,resneg);
    ss(1) = primax;
    ss(2) = primin;
    ss(3) = priabs;
    ss(4) = respos;
    ss(5) = resneg;
    ss(6) = resabs;
    ss(7) = max(respos,primax);
    ss(8) = max(resneg,primin);
    ss(9) = max(priabs,resabs);
    if jtype < 10, s(1,i)=ss(jtype); end
    if jtype == 10, s(:,i)=ss'; end
end

%-------------------------------------------------------------
function y = sdoffltr(b,a,x,zi)
% function y = sdoffltr(b,a,x,zi)
% function filters a time history x with a single-degree-
% of-freedom oscillator using a ramp invarient filter where
% the filter weights [b,a] have been defined by the function
% sdofwt (see sdofwt.m)
%
% reference: sdofwt.m
%
% input: b= a three element vector of filter weights defined
%           by the function sdofwt
%        a= three element vector of filter weights defined
%           by the function sdofwt
%        x= the input vector to be filtered
%        zi= optional intial filter state (see filter), default is [0 0];
%
% output: y= the filtered time history
%
% note: the intial conditions for values of x&y less than index 1
%       are assumed to be zero

% revised 6/9/97 to add optional argument zi
% David Smallwood Sandia National Laboratories Albuquerque NM
% June 6, 1988
% Copywrite 1994-2000 Sandia National Laboratories


if nargin<4,  zi=[0 0]; end
% Edited on 24-Aug-2010 by Chris McLeod
% if length(zi)==0, zi=[0 0]; end
if isempty(zi)==0, zi=[0 0]; end
aa=[a(1) a(2)-2 a(3)+1];
y = filter(b,aa,x,zi);

%---------------------------------------------------------------------------
function [b,a] = sdofwt(fn,sr,z,itype)
% sdofwt.m
% function [b,a]=sdofwt(fn,sr,z,itype)
%
% This routine finds the filter weights for a single-
% degree-of-freedom resonator using a ramp invarient filter.
% The weights are used in conjunction with the function
% sdoffltr and shspec to calculate the shock response
% spectrum of an acceleration time history using a ramp
% invarient simulation.
%
% Reference: 1. Smallwood D.O.,"An Improved Recursive Formula
%               for Calculating Shock Response Spectra," The
%               Shock and Vibration Bulletin 51(2), pp 211-217,
%               May 1981.
%            2. Smallwood D.O.,"The Shock Spectrum at Low Frequencies,"
%               The Shock and Vibration Bulletin 56(1), Appendix A,
%               pp 285-287, Aug 1986.
%
% Input: fn= the natural frequency of the sdof system in Hz
%        sr= the sample rate in samples/sec
%        z = the fraction of critical damping of the sdof system, 0<=z<1
%        itype = >0 sets up a base acceleration, absolute acceleration
%                   response system
%                <0 sets up a base acceleration, relative displacement
%                  (expressed in equivalent static acceleration units)
%                  system
%
% Output: b= three element row vector containing [B0 B1 B2] from ref. 2
%         a=three element row vector containing [1 A1P2 A2M1] from ref. 2

% David O. Smallwood, Sandia National Laboratories, Albuquerque NM
% June 6, 1988
% Copywrite 1994-2000 Sandia National Laboratories

a = zeros(1,3);
b = zeros(1,3);
pi=4*atan(1);
w=2*pi*fn/sr;
a(1) = 1;

if w < .001     % it may be necessary to change this constant
                % depending on the precision of the computer
% use these coefficients when the frequency is small for both
% models
%    small freq
     a(2) = 2*z*w + w*w*(1 - 2*z*z);
     a(3) = -2*z*w + 2*z*z*w*w;
     if itype > 0
%        abs accel model small freq
         b(1) = z*w + (w*w)*( (1/6) - 2*z*z/3 );
         b(2) = 2*w*w*(1-z*z)/3;
         b(3) = -z*w + w*w*( (1/6) - 4*z*z/3 );
     else
%        rel disp small freq
         b(1) = w*w/6;
         b(2) = 2*w*w/3;
         b(3) = w*w/6;
     end
else
%    large freq
     sq = sqrt(1 - z*z);
     e = exp(-z*w);
     wd = w*sq;
     sp = e * sin(wd);
     fact = (2*z*z -1)*sp/sq;
     c = e * cos(wd);
%    a1p2 & a2m1 are the same for both models
%    a1p2=a1+2=a(2)    a2m1=a2-1=a(3)
     a(2) = 2*(1-c);
     a(3) = -1 + e*e;
     if itype > 0
%         large freq abs accel
          spwd = sp/wd;
          b(1) = 1 - spwd;
          b(2) = 2*(spwd - e*cos(wd) );
          b(3) = e*e - spwd;
     else
%         large freq rel disp
          b(1) = ( 2*z*(c-1) + fact + w )/w;
          b(2) = ( -2*c*w + 2*z*(1-e*e) - 2*fact )/w;
          b(3) = ( e*e*(w+2*z) - 2*z*c + fact )/w;
     end
end

%--------------------------------------------------------------------------------
function x = shres(a,t,fn,ZETA)
% shres.m
% function x = shres(a,t,fn,ZETA)
%
% function to calculate the residual shock response spectrum
%   The residual response is the peak response of the sdof system
%   as it decays after the input has ended, i.e., the input is zero.
%   The first two peaks in the decaying response will be the largest.
%   One peak will be negative and one positive.  We don't know before
%   the calculation if the first and largest peak in amplitude will be
%   positive or negative.
% INPUT: a = a two element vector giving the response at two times
%            after the input is zero
%        t = a two element vector giving the times where a was evaluated
%        fn = the natural frequency, inverse units of time, Hz if time is
%             sec.
%        ZETA =  the fraction of critical damping for the sdof filter
% OUTPUT:
%        x = a two element vector giving the amplitude of the response at
%            the first two peaks
% Method:
% The response has the form
% a(t) = exp(-ZETA wn t)[z(1)sin(wd t) + z(2)cos(wd t)].
% If I know a(t) at two values of time, t, I can calculate the constants
% z(1) and z(2).  The general form of the response can then be solved for
% the maximum by:  Finding the time of the first maximum by setting the
% derivative to zero.  Then substituting the time of the maximum response
% back into the general equation to find the maximum response.
% The second peak will occur pi radians later.

% D. Smallwood, Sandia National Laboratories, August 4, 1989
% Copywrite 1994 Sandia National Laboratories
% modified 10/24/96 to fix minor bug in the calculation of tmax;
%     tmax must be between 0 and pi/wd.
% modified 5/13/97 to intialize x.
% modifed 7/8/98 to return zero if input is zero
% modified 8/24/98 to correct some of the error checks, and add warning

   x = [0; 0];
if ZETA>1
   error(['shres: ZETA>1: ZETA = ' num2str(ZETA)])
   x = [0; 0];
   return
end
if ZETA<0
   error(['shres: ZETA<0: ZETA = ' num2str(ZETA)])
   x = [0; 0];
   return
end
if fn<0
   error(['shres: fn<0: fn = ' num2str(fn)])
   x = [0 ; 0];
   return
end

ac = a(:);                    % make sure ac is a column vector
if max(abs(ac))<=eps
   x = [0; 0];
   return
end

if abs(t(2)-t(1))>=1/fn
   x = a;
   warning(['time difference larger than  the period of fn: fn=' num2str(fn) '  t(2)-t(1)=' num2str(t(2)-t(1))])
   return
end
if (abs(t(2)-t(1))>.45*fn) && (abs(t(2)-t(1))< .55*fn)
       x = a;
       warning(['Error in SRS possible: Time difference close to the half the period of fn: fn/2=' ...
               num2str(.5*fn) '  t(2)-t(1)=' num2str(t(2)-t(1))])
       return
end
pi2 = 2 * pi;
fd = fn * sqrt(1-ZETA*ZETA);  % find the damped natural frequency
wd = pi2 * fd;                % damped freq in radians/sec
wn = pi2 * fn;                % natural freq in radians/sec
wdt1 = wd*t(1);
wdt2 = wd*t(2);
e1 = exp(-wn*ZETA*t(1));
e2 = exp(-wn*ZETA*t(2));
c = zeros(2,2);
sd1 = sin(wdt1);
sd2 = sin(wdt2);
cd1 = cos(wdt1);
cd2 = cos(wdt2);
c(1,1) = e1*sd1;
c(1,2) = e1*cd1;
c(2,1) = e2*sd2;
c(2,2) = e2*cd2;
% make sure x and t are a column vectors
xx = x(:);
tt = t(:);
z = c\ac;
piwd = pi/wd;

% the response has the form
% a(t) = exp(-ZETA wn t)[z(1)sin(wd t) + z(2)cos(wd t)]
% the first maximum response will occur at the time, tmax
% tmax = (1/wd) * arctan((wd z(1) - ZETA wn z(2))/(wd z(2) - ZETA wn z(1)))

tmax = (1/wd) * atan( (wd*z(1) -ZETA*wn*z(2))/(wd*z(2) -ZETA*wn*z(1)) );
if tmax<0
   tmax = tmax + piwd;
end
if tmax>piwd
   tmax = tmax - piwd;
end
%  find the maximum

x(1) = exp(-ZETA*wn*tmax)*( z(1)*sin(wd*tmax) + z(2)*cos(wd*tmax) );

% the next peak will occur pi radians later in time

tmin = tmax + piwd;
x(2) = exp(-ZETA*wn*tmin)*( z(1)*sin(wd*tmin) + z(2)*cos(wd*tmin) );

%--------------------------------------------------------------------------------------
